#include "../Random.hpp"
#include <armadillo>
#include <boost/math/distributions/normal.hpp>
#include "../utils.hpp"
#include "LikMS.hpp"
#include "ProbitMSadap.hpp"
//#include "../Misc/Erf.hpp"
#define _Mms 200 

template<class Approx,class APriori,class Likelihoodms>
class Probit_MS : public Density::GeomBridge
{
		friend class Density::GeomBridge;
	public:
		Probit_MS(double *Y, mat X, Distribution::Distribution *S, APriori *P, Likelihoodms *Lik,int ds) : Density::GeomBridge(Y, X, S){ 
			mat s=(*P).Get_s();
			_s=P->Get_s();
			_p=X.n_cols-1;
			_ds=ds;
			_d=new boost::math::normal_distribution<>(0,1);
			Set_p(X.n_cols-1);
			_is=inv(_s);
			_Lik=Lik;
			_m=_Mms;
			fL=new Approx;
			mat m(_p+1,1);
			ds=logfact(_ds);
			d=logfact(_p);
			m.fill(0);
			(*fL)(X,Y,m,_s(0,0),m);			
			_init=fL->Get_Mu();
			_Px=P;
		}
		~Probit_MS(){
			delete fL;
		}
		double Likelihood(mat gamma)
		{
			int p=Get_p();
			mat X=Get_X();
			mat gammat(p+1,1);
			double h=as_scalar(sum(sum(gamma,0)));
			double res=0;
			int nb=h+1;
			int n=X.n_rows;	
			mat X2(n,nb);
			mat init(nb,1);
			//cout << gamma.t();
			//cout << "test"<< p+1;
			int kk=1;
			//gammat on rajoute 1 au debut de gamma
			//X2 les col de X ou gammat==1
			for(int i=0;i<(p+1);i++)
			{	
				if(i!=0){
					gammat(i,0)=gamma(i-1,0);
					if(gamma(i-1,0)!=0)
					{
						X2.col(kk)=X.col(i);
						init(kk,0)=_init(i,0);
						kk++;
					}
				}else{
					gammat(i,0)=1;
					X2.col(0)=X.col(0);
					init(0,0)=_init(0,0);
				}
			}
			//cout << gammat.t();
	
			mat Xt=X*diagmat(gammat);
			mat m(nb,1);
			mat s(nb,nb);
			s.fill(_s(0,0));
			s=diagmat(s);
			m.zeros();
			double *Y=Get_Y();

			(*fL)(X2,Y,m,_s(0,0),m);			
			mat Sig=(*fL).Get_Sig();// approx avec donné Gamma2	
			mat Mu=(*fL).Get_Mu();
			//cout << s;
			//cout << Mu;
			//cout <<"Sig" << log(det(Sig));
			

	
			//Distribution::Gaussian G(Xt.n_cols,m,s);
			Distribution::Gaussian G(nb,Mu,Sig);
			APriori F(nb,m,s);

			mat prop(nb,_m);
			double sum1=0;
			for(int ii=0;ii<_m;ii++)
			{
				prop(span::all,ii)=G.r(1);
			}
			
		//	prop=(G.scrambled(_m)).t();
	//		cout << prop;

			mat beta=add_mat(prop.col(0),gammat,p+1);
			//sum=Lik_Prob(beta,Xt)+F.d(prop.col(0),1)-G.d(prop.col(0),1);
			sum1=Lik_Prob(beta,Xt)+Priorb(prop.col(0))-G.d(prop.col(0),1);
			//	gammat.fill(1);	
			mat sbeta=exp(sum1)*beta;
			mat w(_m,1);
			w(0,0)=exp(sum1);
			mat iSig=inv(Sig);
			for( int i=1;i<_m;i++)
			{
				beta=add_mat(prop.col(i),gammat,p+1);
				//double o=Lik_Prob(beta,Xt)+F.d(prop.col(i),1)-G.d(prop.col(i),1);
			//	cout << beta.t();
				double o=Lik_Prob(beta,Xt)+Priorb(prop.col(i))-dprop(prop.col(i),Mu,Sig,iSig);
	/*			cout << "t1" << Lik_Prob(beta,Xt)<< " ";
				cout << "t2" << Priorb(prop.col(i))<< " ";
				cout << "t3 " << dprop(prop.col(i),Mu,Sig) << " ";*/
				double t=log_add(o,sum1);
				w(i,0)=exp(o);
				sum1=t;
			}

			double ess=0;
			for( int i=0;i<_m;i++)
			{
				ess+=pow(w(i,0)/exp(sum1),2);

			}
			//cout << "ess " << (double)1/ess;
			res=sum1-log(_m);
			//cout << res << "\n";
		//cout << sbeta.t()/exp(sum);	

			return res;

		}

		inline double Priorb(mat theta)
		{
			double res=0;
			mat foo=theta.t();
			//cout << foo << "\n";
			//cout << "//" << bar;
			int d=theta.n_elem;
			//int d=_ds;
			mat ss(d,d);
			ss.fill(_s(0,0));
			ss=diagmat(ss);
			double  bar=as_scalar(foo*inv(ss)*foo.t());
			res=-0.5*d*log(2*PI)-0.5*log(det(ss))-0.5*bar;
			return res;

		}

		inline double dprop(mat theta, mat m, mat Sig,mat iSig)
		{
			double res=0;
			mat foo=theta.t()-m.t();
			//cout << foo << "\n";
			//cout << "//" << bar;
			int d=theta.n_elem;
			//int d=_ds;
			double  bar=as_scalar(foo*iSig*foo.t());
			res=-0.5*d*log(2*PI)-0.5*log(det(Sig))-0.5*bar;
			return res;

		}
		inline double Lik_Prob(mat theta1,mat Xt)
		{
			double* Y=Get_Y();
			double u=(*_Lik)(theta1,Xt,Y);

			return u;

		}
		inline double Phi(double x)
		{
			double t=cdf((*_d),x);
		//	double t=0.5*(1+Erfm(x/sqrt(x)));
			if(t==1)
			{
				t=0.99999999;
			}
			if(t==0)
			{
				t=0.00000001;
			}
			return t;
		}

		double Prior(mat theta){
		    
			double m=0.5;
			double i=sum(sum(theta));
			if(i<=_ds)
			{
				double t=logfact(i)+(logfact(_ds-i))-logfact(_ds)+i*log(m)+(_ds-i)*log(1-m)+logfact(i)+(logfact(_p-i))-logfact(_p);
				return t;
 			}else{
				return -numeric_limits<double>::infinity();

			}		
			
	//		return -0.693;
		}
	
		mat  GradLik(mat theta)
		{
			cout << "Grad ProbitMS deprecated";
			return theta;
		}

	private:
		Approx *fL;
		Likelihoodms *_Lik;
		boost::math::normal_distribution<> *_d;
		Distribution::Distribution *_Px;
		mat _s;
		int _ds;
		int ds;
		int d;
		mat _init;
		int _m;
		int _p;
		mat _is;
	
};
